#include <cusp/linear_operator.h>
#include <cusp/transpose.h>
#include <cusp/gallery/eye.h>
#include <cusp/array1d.h>
#include <cusp/copy.h>

// Version of QR with Householder reflectors
// The output is Q and the *transpose* of R (this is easier to program with contiguous access)



namespace cusp
{

template <typename ValueType> 
struct saxpy_functor : public thrust::binary_function<ValueType,ValueType,ValueType>
{
    const ValueType a;

    saxpy_functor(ValueType _a) : a(_a) {}

    __host__ __device__
        ValueType operator()(const ValueType& x, const ValueType& y) const { 
            return a * x + y;
        }
};

template <typename LinearOperator,
          typename Matrix1,
          typename Matrix2>
__host__ 
int qrhh(LinearOperator& A,
        Matrix1& Q,
        Matrix2& R)
{

  CUSP_PROFILE_SCOPED();

  typedef typename LinearOperator::value_type   ValueType;
  typedef typename LinearOperator::memory_space MemorySpace;

  typedef typename cusp::array1d<ValueType, MemorySpace>::iterator Iterator;

  R = A;
  size_t m = R.num_rows;
  size_t n = R.num_cols;

  cusp::array2d<ValueType, MemorySpace> Qt(m,m);

  cusp::gallery::eye(Qt, m, m);
  cusp::array1d<ValueType, MemorySpace> Quj(m);

  for (size_t j = 0; j < n; ++j)
  {
    cusp::array1d<ValueType, MemorySpace> uj(m-j);

    // pitch is generally = num_cols, but it may be contain buffer space?
    thrust::copy(R.values.begin() + j*R.pitch+j, R.values.begin()+j*R.pitch+m, uj.begin());

    ValueType nuj = cusp::blas::nrm2(uj);

    if (nuj == 0) { return -1; } // null vector; algorithm must terminate softly
    if (uj[0] < 0) 
	uj[0] -= nuj;
    else
        uj[0] += nuj;
    nuj = cusp::blas::nrm2(uj);

    cusp::blas::scal(uj,1/nuj);

    // cusp::print(uj);

    for (size_t i = n; i > j; --i)
    {
        // Update R
        // cusp::blas::dot( uj.begin(), uj.end(), R.values.begin()+j + (i-1)*R.pitch );  DEPRECATED
        ValueType sc = -2*std::sqrt(thrust::inner_product( uj.begin(), uj.end(), R.values.begin()+j + (i-1)*R.pitch, ValueType() ));
        // std::cout << " i " << i << " scalar " << sc << "\n";
        
        // cusp::blas::axpy( uj.begin(), uj.end(), R.values.begin()+j + (i-1)*R.pitch, sc ); // DEPRECATED
        thrust::transform(uj.begin(), uj.end(), R.values.begin()+j + (i-1)*R.pitch, R.values.begin()+j + (i-1)*R.pitch, saxpy_functor<ValueType>(sc));
    }
    //cusp::print(R);
    // Update rows of Q
    for (size_t i = 0; i < m; ++i)
    {
        // cusp::blas::dot( uj.begin(), uj.end(), Qt.values.begin()+j + i*Qt.pitch );   // DEPRECATED
        ValueType sc = -2*std::sqrt(thrust::inner_product( uj.begin(), uj.end(), Qt.values.begin()+j + i*Qt.pitch, ValueType() ));
        // cusp::blas::axpy( uj.begin(), uj.end(), Qt.values.begin()+j + i*Qt.pitch, sc );  // DEPRECATED
        thrust::transform(uj.begin(), uj.end(), Qt.values.begin()+j + i*Qt.pitch, Qt.values.begin()+j + i*Qt.pitch, saxpy_functor<ValueType>(sc));
    }
    //cusp::print(Qt);
  }
  Q=Qt;   // Convert from row-major to column-major ordering
  CUSP_PROFILE_DUMP();
  return 0;
}

} // end namespace cusp

